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TECHNICAL MEMORANDUM 


OPTIMAL CONTROL COMPUTER PROGRAMS 
I. INTRODUCTION 


The theory of optimal control can be better appreciated if simulation tools are available in the 
classroom to supplement class notes. Most optimal control problems, even with low-order systems, are 
not amenable to easy analytical solutions. Therefore, a generic simulation program, if it existed, could 
greatly improve one’s knowledge of the theory. Although commercially available control software is in 
great abundance, few, if any, address the control problem in its most fundamental form, i.e., the first 
necessary condition (FNC) and the maximum principle. The intent of this report is to fill this need for a 
simple simulation tool which would be able to simulate most optimal control problems based on these 
two principles. 


n. STATEMENT OF THE OPTIMAL CONTROL PROBLEM 


The optimal control problem can be simply stated as: Find an admissible control u* which causes 
the system 

x(t) = f{x(t),u{i)J) , (1) 

to follow an admissible trajectory x* and minimizes the performance measure 

rT 

J = G(x(T),T)+\ L(x,t,u)dt . ( 2 ) 

J t 0 


In general, additional constraints may be placed on the control variable u* and the system variable x*. 

Variations in the performance index J lead to many different types of optimal control problems. 
A few typical problems are described briefly in the following sections. 


A. Minimum Time Problem 

Given the time to and the initial state x(to) = x°, the final state is to lie in a specified region S of 
the nxl dimensional state -time space. The objective is to transfer a system from the initial state xP to the 
specified target set S in the minimum time. The performance to be minimized is therefore 

j = t r t Q = I* 1 dt , ( 3 ) 

Jt o 

where ty is the first instant of time when x(t) and S intercept. 



B. Minimum Energy Problem 


The objective of this problem is to transfer a system from a given initial state *(r 0 ) = x° to a 
specified target set 5 with a minimum expenditure of energy. The performance index in this case will 
then be 

J= P 1 [u T (t)Ru(t)}dt , (4) 

J / 0 

where R is a positive definite constant matrix. 

C. State Regulator Problem 

The objective is to transfer a system from the initial state x(fo) = x° to the desired state x d with the 
minimum integral square error. Relative to the desired x d , the quantity (x(r)-^) can be viewed as the 
instantaneous system error. If the system coordinates are transformed such that x 4 becomes the origin, 
then the new state x(t) is itself the error. 

For the state regulator problem, a useful performance measure is therefore: 

rT 

J = ±x r {T)Hx{T) + ±^ {x T (t)Qx(t)+u T (t)Ru(t)}dt , (5) 

where Q is a constant matrix not required to be positive semidefinite. 1 

If the terminal time is not constrained, then x d = 0 (assuming a stable system). In which 

case, the performance index will be 

J = X f {x T (t)Qx(t)+u T (t)Ru(t)}dt . (6) 

1 Jt 0 

An extension to the state regulator problem is the output regulator problem, where the state error 
is replaced by output error y(t), then 

J = 2 I" \y T (t)Qy{t)+u T (DRu(t)}dt . (7) 


D. Tracking Problem 

The objective of the tracking problem is to maintain the system state x(t) as close as possible to 
the desired state r(t) in the interval [to,T\. Therefore; 


where 


J = - e T (T)He(T) + - C {e T (t)Qe(i)+u T (t)Ru(t)}dt , (8) 

2 2J» 0 

e(t) = (x(t)-r(t)) (9) 
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III. SOLVING OPTIMAL CONTROL PROBLEM 


Solutions to the optimal control problem can be categorized into three separated approaches. The 
first one is based on the calculus of variation from which the FNC is derived. The second one is based 
on the Pontryagin’s maximum (minimum) principle when the control effort is constrained. The third 
effort is based on the concept of dynamic programming and the principle of optimality. 

The theoretical background of these approaches is discussed in most optimal control textbooks 
and, therefore, is not discussed in this paper. The procedure of these methods, however, is the foundation 
of the numerical methods. 


A. FNC Algorithm 

The FNC algorithm can be summarized in the following; for the plant 


x = f(x,t,u); f = 




\fnj 


( 10 ) 


and the performance index 


J = G(x(T),T) + f r L(x,t,u)dt , (ID 

Jt 0 

and the boundary conditions (x(to),to) given {x(T),T) e 3, where u is unbounded, piecewise continuous 
scalar control and G,L are real-valued, sufficiently smooth scalar functions, 3 is the given m-dimen- 
sional “terminal manifold.” Then, the FNC for the optimal control u°(t) and the associated optimal tra- 
jectory jt°(0 can be determined by the following procedure: 

(a) Define Hamiltonian H\ 


H = H(x,p,t,u) stef X Pifi(x,t,u)-L(x,t,u ) . 


( 12 ) 


(b) The first integral condition is 


dH 

du 


= 0 


0 °/ *\ 
U = M ( X,p,t ) 


u = u 


( 13 ) 


(c) Define: 


H°(x,p,t) MH(x,p,t,u°(x,p,t)) . 


( 14 ) 


3 


(d) Form the Euler-Lagrange equations: 


x 0-Ml 
' " M ’ 

». o = ^ 

Pl dX; ’ 

(e) At terminal time 7°, the transversality condition 

[h°(T 0 )- $£] dt I rtf- X a<T°) + dx;\ rr = 0 , 

must be satisfied for every perturbation {dt, dx \, .... dx n ) in the tangent plane to 3 at the point 
(XP{T°),T°). This procedure leads to 2n+l conditions with 2n+\ unknowns. 


Example 1 


x = ax+u , 


J = x(T) + 

(. “’W* • 

( T 

= fixed; 

3 ={x(T) 

= free; 

U* 0 ) 

= given . 


Solution: 


H = pax+pu-u 2 , 

M = p -2u = 0^«0 = f . 


Then the optimal Hamiltonian becomes; 


H ® = pax + . 


From the Euler-Lagrange equations, the following state and co-state equations were derived: 


, dH 


x = 


df = aX+ 2 P ’ 


. a // 0 

p= —dT = - pa 


(15a) 

(156) 

(16) 
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Applying TV condition to the problem results in the following boundary conditions: 

{x(r 0 ) = given, p 1 (T) = -\ . T = given} . 

This is a two-point boundary condition problem that can be solved by one of the programs 
developed in this paper. 


B. Pontryagin’s Minimum Principle 

In the case of the closed and bounded control region, the optimal control u°(x,p,t ) is found by 
minimizing H{x,u,p,t) with respect to controls w in the given controls region U, while treating the other 
variables as constants. In other words, u°(x,p,t ) is the admissible control vector for which H(x,u,p,t ) has 
its minimum value. The minimum principle procedure for optimal control can be summarized as: 

For the plant 


x 


f(x,t,u ); / = 


'js' 


\fnj 


(17) 


and the performance index 


J = G(x(T),T) + ( T L(x,t,u)dt , (18) 

Jt o 

and the boundary conditions ( x(to),to,x(T )) given we U for all re [fo.TJ. 

(a) Form the Hamiltonian 

H = H(x,p,t,u ) dsf X Pifj(x,t,u)-L(x,t,u ) . (19) 

1 

(b) Find the optimal control w when it is not saturated 


= u°(x,p,t) . (20) 


u°>U; 

lw°l < U; (21) 

u°<-U . 


dH | 
3 ^ 


= 0 — > w 


(c) Then the optimal control w°(r) is 


U for 
w °(t) = { u °(f) for 
■U for 
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(d) Solve the set of 2 n equations 


o-Ml 

' " 3a ’ 



-dH° 

dxj 


(22 a) 
(22 b) 


Example 2 
Plant 


x l (t) = x 2 (t) , 
x 2 (t) = -x 2 (t)+u(t) . 

The performance index to be minimized is 

J = ^ j l (x}+u 2 )dt . 

The control constraints are given by 

lw(r)! ^1 for te [r 0 ,til • 

The Hamiltonian in this case is 

H(x,U,p,t) = ±x} + ±U 2 +PiX 2 -P2X2+P2 U • 

To determine the control that minimizes H subject to the inequality constraints, we first separated all of 
the terms containing u{t) 

1 ? 


When the control is unsaturated, we have 


ML = 0=>u*(t) = -p 2 . 

du 

Thus, the optimal control in this case is 

f - 1 for p 2 (t) > 1; 
u*( y ) = / -p 2 for \p 2 \< 1; 

y +1 for p 2 (t) < -1 . 

Therefore, in order to determine u(t) explicitly, state and co-state equations must be solved subject to the 
given boundary conditions. 
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C. Dynamic Programming Approach 3 


The dynamic programming approach to optimal control was derived by Bellman (1962) from his 
principle of optimality. The algorithm for the linear time-invariant (LTI) system is derived in this sec- 
tion. Let us consider a discrete LTI system described by the matrix-vector difference equation 

x k+x = Ax k +Bu k . (23) 

We would like to find a sequence of control vectors, uq,u\,...us-\, to minimize a performance 
index or cost function 

i 1 N ~ l 

J = ^x$Hx n + j ^x][Qx k +u][Ru k , (24) 

where H and Q are symmetric nXn matrices, R is an mxm positive definite matrix, and N is a fixed 


integer. Then the cost at the end point N is 

Jn,N = j x 1jHx n = j xJ]P N x N . (25) 

It is obvious from the definition that /fy = H. The cost over the last two points is 

Jn-W = 2 x l-\Q xN ~ 1 + 2 M Jf-i Ru R-i + 2 x ^ P n x n > ( 26 ) 

and the minimum cost at this point is 

= . (27) 

After substituting the state equation x n = Ax n _ x +Bu N _ x into equation (26), the new equation becomes 

Jn- ijt = min xJi_ x Qx n - 1 + £ u Ji^Ru^ + ± (Ax^+Bu^) T P N (Ax N _ l +Bu N _ l )} , (28) 

minimizing this with respect to tfv-i gives 

= 0 = u l^Ax^Bu J,_,)P„B . (29) 

Therefore, the optimum control effort at AM stage is 

« jv-i = ~(R+B T P N B)~ l B t P n Ax n _ x . (30) 

Clearly this is negative feedback control which is proportional to the system state or 

« n-i = ~Fn-i x n - i • (31) 
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Repeating the same procedure backward in stages, a recursive algorithm can be developed as in the 
following: 


F k _ x = [R+B T P k B] ~ l B T P k A , 

(32) 

= A T P i £-fI x {R+B T P i P)F^+Q . 

(33) 


This algorithm is implemented in the computer program. 


IV. COMPUTER SOLUTION OF OPTIMAL CONTROL PROBLEM 


A. Continuous Time Systems 

The two-point boundary value problem derived from the FNC can be solved by several 
algorithms. The steepest descent method, Fletcher-Powell method, and the shooting method are a few 
very common ones. In this report, the steepest descent method was implemented because of its ease and 
simplicity. The algorithm of the maximum descent method in this application is shown in figure 1. In 
order to understand this approach, it is best to illustrate it with an example. 

Example 3 

Plant (state equations): 


x j(t) = -2[jc ,(t)+0.25]+[x2(t)+0.5] exp - [xj(/)+0.25]«(r) , 

x 2 (t) = 0.5-x 2 (r)-[x 2 (r)+0.5] exp - 

x(0) = [0.05 0] T , R = 0.1 . 


Performance index: 


= [x ^(f)4* 2 (f)+0. 1 u 2 (r)]df 


The Hamiltonian is: 

H(x(t),u(t),p(t)) = x^+xl+Ru 2 +p l (t) 


-2[jcj(r)+0.25]4{x 2 (r)+0.5] exp - [* i(0+0.25]u(r)j 


+ Pj(t) 


0.5-x 2 (r)-[x 2 (f)+0.5] exp ■ 
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Co-state equations: 


P[(t) = -2x l (t)+2p l (t)-p l (tpc 2 (t)+0.5] 


50 

[x x (t)+2\ 2 


exp 


' 25£|(f) ' 

X\(t)+2 


+p x (t)u(t)+p 2 (t\x 2 (t)+0.5] 


50 1 [25x 1 (t)' 

i^r p teJ 


p 2 {t) = -2x 2 {t)-p x {t) exp 


25 £ i (t)' 

X\(t)+2 


+ Pi(t) 



25*iMl\ 

x l (t)+2\f 


The optimal control u*(t) was calculated from: 

^ = 0.2u(t)-p l (tlx l m0.25) = 0 . 


The norm used was 



and the iteration procedure is terminated when 


dH 

du 



(34) 


(35) 


In the steepest descent method, the step size r is determined by checking the performance J at 
each major iteration. If J calculated in the current iteration is greater than the previous iteration, the r is 
halved, and the calculation is repeated before moving on to the next iteration. The results of the simula- 
tion employing the steepest descent method are shown in figures 2 through 4. The optimal control u* 
and state trajectories x\{t),x 2 {t) and their intermediate values are given to show convergence. Table 1 
shows the convergence comparison for various initial guesses on u( 0) and on r. 


This was a problem with free terminal conditions. Another problem with fixed terminal condi- 
tions can be demonstrated by using the same program. 


Example 4 


Plant model: 


x x =x 2 


x 2 = u 


Performance index: 


J = 


±x(T) t Hx(T) + ± 
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The terminal conditions being: 


*(0) = [1 l] r ; x(T) = [0 0] r ; T = 10 . 
The co-state equations can be found from the Hamiltonian and are 

A = ° 

Pi~~P\ • 


The optimal control u(t)* was calculated from: 

&i = u(t)+p 2 (t) = 0 . 


The norm used was 


ML 

du 


2 



The termination criterion remained the same when the norm reached 10 -2 . The two cases of H were 
simulated to demonstrate the effect of weighing on the terminal conditions. Figures 5 through 1 1 show 
the results of this simulation. 

A special case of the continuous time optimal control problem is the linear quadratic regulator 
(LQR) problem. The LQR problem was solved by Kalman 4 and can be summarized in the following: 

For the pla nt 


x(t) = A(t)x(t)+B(t)u(t) ; x(t 0 ) = x° , 


(36) 


and the performance index is 

J = jx T (t l )Hx(t l ) + { x T (t)Q(t)x(t)+u T (t)R(t)u(t)}dt , (37) 


the optimal control law is 


u*(t) = K(t)x(t) , 


(38) 


where the feedback gain is 

K(t)=-R-\t)B T (t)P(t) , (39) 

-Pit) = Q(t)-P(t)B(t)R-\t)B T (t)P(t)+P(t)A(t)+A T (t)P(t) , (40) 

P(t l ) = H , (41) 
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the optimal performance index is 


J* = ±x T (t)P(t)x(t) . (42) 

This procedure is developed for generic, time-varying LQR problems. A subclass of this is the 
steady-state LTI LQR. In computational terms, this can be accomplished by integrating the P matrix 
until steady state is reached. The following example (Kirk) will serve to illustrate this technique. For the 
plant model: 

x l (t) = x 1 {t) , 
x 2 {t) = 2x l (t)-x 2 (t)+u(t) , 
r T 

J = \o + • 

The A, B, Q, and R matrices are then 



The results of the simulations are shown in figures 12 and 13. 

B. The Discrete Time LQR Problem (5) 

Consider the discrete time dynamic system described by the vector-matrix difference equation 


x(k+l) = Ax(k)+Bu(k), k = 0,l,...,N-\ . (43) 

The objective is to find a sequence of control vectors, w(0),m(1),...,k(N- 1), to minimize the 
following performance index: 

i W-l , , 

J = \ x T {N)Hx(N) + X 4 x T (k)Qx(k) + 4 u T (k)Ru(k) . (44) 

Z Jc = 0 Z Z 

The derivation of the feedback control law can be found in reference 3. The recurring feedback 
gain calculation is summarized in the following: 

u(k) = -F(k)x(k) , (45) 

F(k) = R~ l B r {A T T\P{k)-Q) , (46) 

G(k+\) = P(k+l)-P(k+\)F{B T P(k+\)B+R]~ l B T P(k+l) , (47) 

P(k) = A T G(k+l)A+Q . (48) 
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The terminal condition is 


P(N) =H . 


(49) 


Given the terminal condition, we can evaluate G(N), P(N-l), and F(N-l) and continue cyclically 
until the gain matrix is evaluated at all points in time to yield F(0),F(1),F(2),...,F(AM). 

As an illustrative example, let us consider a second-order discrete system with the following 
plant dynamics and performance index: 


'*!(*+ 1 )‘ 



. 


It 


0.6277 0.3597 1 1 * i<*) 1 + \ 0.025 


[0.0899 0.8526JI jc 2 (fc) I 10.115 


«(*) , 


;4l x T (k)Qx(k)+Ru 2 (k) , 
2- k = o 


Q = 


50 0 
0 10 




Since there is no terminal penalty, then 



The discrete feedback gain and the optimal state trajectories are shown in figures 14 through 16. 


C. Dynamic Programming Approach to Optimal Control 

The theory of dynamic prog ramming was introduced by Bellman (1957). Although the theory 
was primarily developed for the solution of certain problems by a digital computer (which implies 
discrete-time data), it has been extended to continuous time analysis. 

Consider a process described by the state equations 

x(k+ 1) = f(x(k),u(k)Jc ) ; * e [0, AM] . (50) 

We shall be interested in selecting the control u(k)\ k = 0,1,..JV-1 which minimizes a performance func- 
tion of the form 


N - 1 

J = h(x(N)M + Z g(x(k),u(k)Jc) . (51) 

This process is called the multistage decision process of N stages where the choice of u(k) at each 
sample instant is considered the decision of interest. A recurring relationship for the optimal decision 
u(k) is 


u(k) = -F(k)x(k) , 


(52) 
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(53) 


F(k-l) = [R+B T P(k)B]- l B T P(k)A , 

P(k - 1 ) = A r P(k)A-F T (k-\)(R+B T P{k)B)F{k-\)+Q , (54) 

P{N) = H . (55) 

A very important result of the dynamic programming approach is the idea of obtaining the total 
cost over the entire stages, and it is given as: 

Jq,n =j x T (0)P(0)x(0) . (56) 

Using the same example as in section B, the feedback gain and the state trajectories calculated by 
using this algorithm are shown in figures 17 through 19. 

The method of dynamic programming can be extended to a continuous-time system based on the 
multistage decision process as described in reference 2. A natural approach is to replace the continuous- 
time problem by its finite difference approximation. Then the results of equations (50) to (56) can be 
readily applied. It is of interest to compare the results obtained from the discrete LQR with that from the 
dynamic programming approach. Figures 20 through 24 show the comparison of the state trajectories, 
optimal feedback gains, and the optimal control effort. 


V. CONCLUSION 


In this report, several computer programs were developed that provide simulation tools for a 
large class of optimal control problems. The programs written are very simple in nature. They are all 
based on the manipulations of a few subroutines for matrix operations. Although the numerical examples 
shown were of second-order systems, there is no reason why a higher-order system cannot be simulated 
by appropriate changes in array dimensions. It is hoped that these programs will be of value to engineers 
to develop a “feel” for some optimal control problems at hand. 
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Table 1. Convergence comparison for steepest descend. 


Initial 

Control 

Initial 

X 

Number of 
Iterations 

Minimum 

J 

Final 

X 

Stopping 

Criterion 

1.0 

5.0 

8 

0.03166 

0.3125 

Norm 

1.0 

1.0 

40 

0.03184 

0.25 

Norm 

1.0 

0.25 

92 

0.03185 

0.125 

Norm 

-1.0 

0.25 

161 

0.03186 

0.0625 

Norm 

-1.0 

1.0 

28 

0.03180 

0.5 

Norm 

0 

5.0 

6 

0.03163 

0.3125 

Norm 


Initial Guess 
u(‘\i = 0 


Integrate ' 
forward 
x — f(x,u,t) 


Calculate 

costate 

B.C. 


Integrate 
backward 
• __ dH 


■■‘-'iff 


Increment 

counter 


J i+ 1 < J< 7 s 




Output 


Figure 1. Steepest descend algorithm for two-point boundary value problem. 
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Figure 8. Optimal control effort with relaxed terminal penalty. 
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Figure 9. Optimal trajectories comparison. 
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Two-Point Boundary Value Solver 


program twopt 
c 

c This program solves a generalized two point value problem 

c derived from the First Necessary Condition of the 

c optimal control theory based on calculus of variation, 

c The method of maximum descent in conjunction with 4th order 

c runge-kutta routine is used. 

c basically, it integrates the state equations forward in time 
c and set the boundary condition of the co-state equations 

c according to the Transversality Condition, 

c The co-state equation is then integrate backward in time, 

c The performance index is checked every iteration, 

c xdot and x are state derivative and state respectively 

c pdot and p are co-state derivative and co-state respectively 

c np = total number of points (np*dh=T=total simulation time) 

c dh = integration step time 

c n = number of states 

c m,k = runge-kutta routine indeices, set to 0 when initially called 
c subroutine called 

c runge (n, x, xdot, t , dh,m,k) 

c For higher order problems, change the dimension appropriately 

c This problem simulates the example on page 12 of the paper 

c 

real x(2) , xdot (2 ) ,p (2) , pdot (2) , xl (100) , 

* dhdu (100) ,u(100) , hold (100) , h{100) , pi (100) , x2 (100) ,p2 (100) 
open (unit=3 , f ile= ' hwl . out ' , status= ' old' ) 

open (uni t=4 , file= ' hw2 . out ' , status= ' old' ) 
tau = 1 . 
c 

c store initial guess of u 

c 

do 1 j=l,np 
u(j) = 1. 

1 continue 
111 n = 2 
t = 0. 
dh =.02 
np=7 8 
m=0 

x ( 1 ) =0 . 05 
x ( 2 ) =0 . 
c 

c Integrate state equations forward 
c 

do 8 i = 1 , np 

6 call runge (n,x, xdot , t , dh,m, k) 
goto (10 , 20) ,k 
c 

c insert state equations here 

c 

10 xdot ( 1 ) = -2.*{x(l)+0.25)+(x(2)+0.5) *exp( (25.*x{l) ) / (x(l) +2 . ) ) 

* - (x (1) +0 . 25) *u (i) 

xdot (2)=0.5-x(2)-(x(2)+0.5) *exp ( (25 .*x{l) ) / (x(l) +2 . ) ) 
goto 6 
c 

c Store state trajectories 

c 

20 xl(i) = x ( 1 ) 
x2(i) = x{2) 

c print *,i,x(l) ,x(2) ,k 

8 continue 
c 

c set initial condition for co-state equation 

c and find hamiltonian h 

c 

D(l) =0. 
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p(2)=G. 
m = 0 

do 11 i 1 = 1 , np 
11 hold ( il ) = h(il) 
c 

c integrate co- state equations backward 

c 

do 80 i2=np,l,-l 

c print * , pdot { 1 } ,pdot (2) , p ( 1 ) / p ( 2 ) 

60 call runge(2,p,pdot, t,dh,m,kk) 
goto (100 , 200) , kk 
c 

c insert co-state equations here 

c 

100 duml=x2(i2) + 0.5 

epl=50 ./{xl(i2)+2)**2 
ep2=exp (25 . *xl (i2)/(xl(i2)+2.)) 

pdot (1) = 2 . *xl (i2) -2 . *p (1) +p(l ) *duml*epl*ep2-p (1) *u(i2) 

* -p (2) *duml*epl*ep2 

pdot (2) = 2 . *x2 (12 ) +p (1) *ep2-p (2 ) * (1 .+ep2) 
goto 60 
c 

c calculate hamiltonian and dhdu 

c 

200 h(i2 ) = 0 . 5*u (i2 ) - p(l)*xl(i2) + p(l)*u(i2) 
dhdu { i 2 ) = 0.2*u(i2)-p(l)*(xl(i2)+0.25) 
pl(i2) = p(l) 
p2 ( i 2 ) -p ( 2 ) 

80 continue 
c 

c check convergeance and define new u(ik) 

c sum = the integral of the norm used for convergeance checking 

c sumj = performance sum for checking that J is indeed decreasing 

c otherwise reduce tau 

c icount = number of iteration desired, can arbitrarily set. 

c 

sum j old = sumj 
sum = 0 * 
sumj =0 . 
do 4 i3=l,np 

u(i3) = u(i3) - tau* dhdu(i3) 
sum=sum + dhdu(i3)**2 

sumj = sumj + (xl(i3)**2 + x2 (i3 ) **2+0 . l*u (13) **2 ) *dh 
4 continue 

if (sumj .It. sumjold) goto 14 
tau = 0 . 5*tau 
goto 111 

14 write (4 ,44) icount , sum, sumj , tau 
44 format ( ilO , 3f 15 . 8) 
icount = icount + 1 
if (icount .ge. 200) goto 5 
if (sum .gt. .01) goto 111 
c 

c after optimal control is found, the state trajectories 

c are recalculate based on stored u 

c 

x ( 1 ) =0 . 05 

x(2) =0 . 

do 88 i4=l,np 

66 call runge (n,x,xdot, t, dh,m,k) 
goto (110,88) , k 

110 xdot(l) = -2.* (x(l)+0.25)+(x(2)+0.5) *exp( (25.*x(l) ) / (x(l)+2 . ) ) 

* - (x(l) +0 . 25) *u(i4) 

xdot (2 ) =0 . 5-x(2 ) - (x (2 ) +0 . 5) *exp ( (25 . *x(l) ) / (x(l) +2 . ) ) 
goto 66 
88 continue 
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c 

c 

c 


outputs to data file for plotting 


5 do 1000 iz=l,np 

1000 write (3 , 1001) i count, xl (iz) , x2 (iz) ,dhdu(iz) ,u(iz) 

1001 format ( i4 , 4 f 10 . 4 ) 
stop 

end 

subroutine runge{n,y, f , t ,h,m,k) 
dimension y(2),f(2),q(2) 
m= m+1 

gotod, 4,5,3,?) , m 

1 do 2 i = 1 , n 

2 q(i ) = 0. 
a = .5 
goto 9 

3 a=l. 707107 

4 t = t + ,5*h 

5 do 6 i = 1 , n 

y (i ) = y(i) + a*f (i) *h-q(i) 

6 q(i) = 2 . *a*f (i) *h ( 1 . -3 . *a) *q (i ) 
a = .2928932 

goto 9 

7 do 8 i = l , n 

8 y(i) = y(i) + h*f (i) /6.-q(i) /3 - 
m = 0 

k = 2 
goto 10 

9 k =1 

10 return 
end 
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Continuous Time Matrix Riccati Equation Solver Application to 
Time-Varying Optimal Control 

program clqr 
c 

c This program presents a algorithm for solving continous 

c time LQR problem by solving time varying Matrix Riccati 

c equation. 

c This program is baseb on many subroutines for matrix 

c manipulations. 

c Euler integration routine is used for simplicity 

c np = number of integration steps 
c np*dt = terminal time 

c This routine can also integrate for large np for steaty 
c state (or infinite time LQR) problems. 

c This program simulates the example on page 15 of this paper 

real a ( 2 , 2 ) , at (2 , 2 ) , q(2 , 2) , p (2 , 2 ) , r ( 1 , 1) , ri { 1 , 1 ) ,pdot{2,2) 
real b (2 , 1) ,bt (1 , 2) , pa ( 2 , 2 ) , atp (2 , 2 ) , rbp (1 , 2 ) ,pbrbp(2,2) 
real pbrbp2 (2,2) ,u(l,l),x(2,l),h(2,2) ,brbp(2,2) ,pbrbpl (2,2) 
real up ( 1 , 1 ) , ax ( 2 , 1 5 , bu (2 , 1 ) ,xdot(2,l) ,pll(1000) ,p!2 (1000) 
real p21(1000) ,p22(1000) 

open (unit =3 , f ile= ' clqr . out ' , status= 'old 7 ) 

1 = 2 
m=l 

np=150 

c 

c define a,b,q,h matrices 

q (1 , 1) =2 . 
q( 1 , 2) =0 . 
q(2,l)=0. 
q(2,2)«l. 

a ( 1 , 1 ) =0 . 
a ( 1 , 2 ) =1 . 
a (2 , 1) =2 * 
a (2 , 2) =-1 ♦ 
c 

b (1 , 1) =0 . 
b(2 , 1) =1 . 
r (1 , 1) =0 . 5 
dt =.l 
c 

h(l , 1) =0 • 
c 

c set initial state value x 

x ( 1 , 1) =-4 , • 

x(2 , 1) =4 . 
c 

c find constant transpose and inverse 
c 

call matran (b,bt , 1 ,m) 
call matran (a, at, 1,1) 
call matinv (r , ri ,m,m) 
c 

c set p final to h 

c 

do 1 ix=l,l 
do 2 iy=l,l 
p(ix,iy) = h(ix, iy) 

2 continue 

1 continue 

do 200 ii=np / l,-l' 
c 

c find f (k) = [R + Bt P B] A -1 Bt P A 

c 

call matcnial (ri , bt , d , rbo ,m, 1 , 1 ) 
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call matmul (b, rbp,brbp, 1 ,m, 1) 

call matmul (p, brbp, pbrbp, 1 , 1 , 1) 

call matmul (p, a, pa, 1,1,1) 

call matmul (at,p,atp, 1,1,1) 

call mat add (pa, pbrbp, pbrbp 1 , 1, 1 , -1 . ) 

call mat add (pbrbpl , atp, pbrbp2 ,1,1,1.) 

call mat add (pbrbp2 , q, pdot ,1,1,1.) 

call euler (pdot , p, dt , 1 , 1 ) 

pll (ii) =p ( 1 , 1) 

pl2 (ii) =p( 1 , 2) 

p21 (ii) =p( 2 , 1) 

p22 (ii ) =p ( 2 , 2) 

200 continue 
time = 0 . 
do 300 ij=l,np 
time = time + dt 
p(l,l)=pll(ij) 
p ( 1 , 2 ) =pl2 (ij) 
p ( 2 , 1 ) =p21 (ij) 
p(2,2)=p22 (ij) 

call matqual (ri , bt , p, rbp,m, 1,1) 
call matmul (rbp , x, up, m, 1 ,m) 
call matneg (up, u,m,m) 
call matmul (a , x, ax, 1 , 1 , m) 
call matmul (b,u,bu, 1 ,m,m) 
call matadd(ax,bu,xdot, 1 ,m, 1 . ) 
call euler (xdot , x, dt , 1 ,m) 

300 write ( 3,301) time, p (1,1) ,p(l,2) ,p(2,2) ,x(l,l) , x (2 , 1 ) ,u (1 , 1) 

301 format (7el5. 5) 
stop 

end 

subroutine euler (dot ,pp, dt , lr, 1c) 
real dot(lr,lc) ,pp(lr,lc) 
do 10 i=l,lr 
do 20 j=l,lc 

pp(i, j)=pp(i, j) + dot ( i , j ) *dt 
20 continue 
10 continue 
return 
end 

subroutine matneg(mata,matb, lr,lc) 
real mata ( lr , lc) ,matb(lr , lc) 
do 10 ix=l,lr 
do 20 iy=l,lc 

matb(ix,iy) = - mata(ix,iy) 

20 continue 

10 continue 

return 
end 

subroutine matqual (mata,matb,matc,matd,mm, 11,11) 

real mata(mm,mm) ,matb (mm, 11 ) ,matc(ll,ll) ,matd(mm,ll) ,dum(l,2) 

call matmul (matb,matc, dum,mm, 11,11) 

call matmul (mata , dum,matd, mm, mm, 11 ) 

return 

end 

subroutine matinv ( a , ai , n,m) 
real a (n,m) ,ai(n,m) ,aa(2,2) 
do 10 i = 1 , n 
do 20 j=l,m 
aa(i, j)=a(i, j) 

20 continue 

10 continue 

call sgefa (aa, n,m, ipvt , info) 
call sgedi (aa, n,m, ipvt , det , work, 01 ) 
do 1 i i = 1 , n 
do 2 ii=l,m 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


ai (ii , i j ) =aa (ii , i j ) 
continue 
continue 
return 
end 

subroutine sgedi (a, Ida, n, ipvt , det , work, job) 
integer Ida, n, ipvt (1) , job 
real a ( Ida, 1) , det (2 ) , work (1) 

sgedi computes the determinant and inverse of a matrix 
using the factors computed by sgeco or sgefa. 

on entry 


a 

real (Ida, n) 

the output from sgeco or sgefa. 

Ida 

integer 

the leading dimension of the array a . 

n 

integer 

the order of the matrix a . 

ipvt 

integer (n) 

the pivot vector from sgeco or sgefa. 

work 

real (n) 

work vector. contents destroyed. 

job 

integer 

= 11 both determinant and inverse. 

= 01 inverse only. 

=10 determinant only. 

on return 


a 

inverse of original matrix if requested, 
otherwise unchanged. 

det 

real (2) 

determinant of original matrix if requested, 
otherwise not referenced, 
determinant = det(l) * 10.0**det(2) 
with 1.0 .le. abs(det(l)) .It. 10.0 
or det(l) .eq. 0.0 . 


error condition 


a division by zero will occur if the input factor contains 
a zero on the diagonal and the inverse is requested, 
it will not occur if the subroutines are called correctly 
and if sgeco has set rcond .gt. 0.0 or sgefa has set 
info .eq. 0 . 

linpack. this version dated 08/14/78 . 

cl eve moler, university of new mexico, argonne national lab. 

subroutines and functions 

bias saxpy , sscal , sswap 
fortran abs,mod 

internal variables 

real t 
real ten 


36 



integer i , j , k, kb, kpl , 1 , nml 
c 
c 

c compute determinant 

c 

if (job/10 . eq. 0) go to 70 
det(l) = l.OeO 
det (2 ) = O.OeO 
ten = lO.OeO 
do 50 i = 1, n 

if (ipvt(i) . ne. i) det(l) = -det(l) 
det(l) = a(i, i) *det (1) 
c ... exit 

if (det(l) .eq. O.OeO) go to 60 
10 if (abs(detd)) .ge. l.OeO) go to 20 

det(l) = ten*det(l) 
det (2) = det ( 2 ) - l.OeO 
go to 10 
20 continue 

30 if (abs(det(l)) .It. ten) go to 40 

det (1) = det (1) /ten 
det (2 ) = det (2) + l.OeO 
go to 30 
40 continue 

50 continue 

60 continue 

70 continue 
c 

c compute inverse (u) 

c 

if (mod(job,10) .eq. 0) go to 150 
do 100 k = 1, n 

a(k,k) = 1 . OeO/a (k, k) 
t = -a (k, k) 

call sscal (k-l,t,a(l,k) ,1) 
kpl = k + 1 

if (n .It. kpl) go to 90 
do 80 j = kpl, n 
t = a ( k , j ) 
a(k,j) = O.OeO 

call saxpy (k, t,a(l,k) ,l,a(l,j) , 1) 



80 

continue 


90 

continue 


100 

continue 

c 



c 


form inverse (u) ^inverse (1) 

c 


nml = n - 1 


if (nml .It. 1) go to 140 
do 130 kb = 1, nml 
k = n - kb 

kpl = k + 1 

do 110 i = kpl, n 
work(i) = a(i,k) 
a(i,k) = O.OeO 
110 continue 

do 120 j = kpl, n 
t = work ( j ) 

call saxpy (n, t,a(l, j) , l,a(l,k) ,1) 

120 continue 

1 = ipvt(k) 

if (1 .ne. k) call sswap (n,a (l,k) , 1, a (1 , T) , 1 ) 
130 continue 

140 continue 

150 continue 
return 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 
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c 

c 

c 

c 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


c 

c 

c 

c 


c 

c 

c 


end 

subroutine sgefa (a , Ida, n, ipvt , info) 
integer Ida, n, ipvt (1) , info 
real a{lda,l) 

sgefa factors a real matrix by gaussian elimination. 

sgefa is usually called by sgeco, but it can be called 
directly with a saving in time if rcond is not needed. 

(time for sgeco) = (1 + 9/n)*(time for sgefa) 

on entry 

a real (Ida, n) 

the matrix to be factored. 

Ida integer 

the leading dimension of the array a . 

n integer 

the order of the matrix a . 

on return 

a an upper triangular matrix and the multipliers 

which were used to obtain it. 

the factorization can be written a = l*u where 
1 is a product of permutation and unit lower 
triangular matrices and u is upper triangular. 

ipvt integer (n) 

an integer vector of pivot indices. 

info integer 

= 0 normal value. 

ss k if u(k,k) .eq. 0.0 . this is not an error 
condition for this subroutine, but it does 
indicate that sgesl or sgedi will divide by zero 
if called, use rcond in sgeco for a reliable 
indication of singularity. 

linpack. this version dated 08/14/78 . 

cleve moler, university of new mexico, argonne national lab. 

subroutines and functions 

bias saxpy , sscal , isamax 

internal variables 

real t 

integer isamax, j , k, kpl , 1 ,nml 


gaussian elimination with partial pivoting 

info = 0 
nml = n - 1 

if (nml .It. 1) go to 70 
do 60 k = 1, nml 
kpl = k + 1 

find 1 - pivot index 

1 = isamax (n-k+1 , a (k , k) , 1 ) + k - 1 

iuvt (k) = 1 


38 



c zero pivot implies this column already triangularized 

c 

if (a(l,k) .eq. O.OeO) go to 40 

c 

c interchange if necessary 

c 

if (1 .eq. k) go to 10 
t = a (1 , k) 
a(l,k) = a (k, k) 
a(k,k) = t 
10 continue 

c 

c compute multipliers 

c 

t = -1 . OeO/a (k,k) 

call sscal (n-k, t , a (k+1 , k) ,1) 


c 

c 

c 


c 

c 

c 

c 

c 


c 


c 

c 

c 

c 


row elimination with column indexing 

do 30 j = kpl, n 
t = a (1 , j ) 

if (1 .eq. k) go to 20 
a(l, j ) = a (k, j ) 
a(k, j ) = t 

20 continue 

call saxpy (n-k, t , a (k+1 , k) , 1 , a (k+1 , j ) ,1) 

30 continue 

go to 50 
40 continue 

info = k 
50 continue 

60 continue 
70 continue 
ipvt(n) = n 

if (a(n,n) .eq. O.OeO) info = n 

return 

end 

subroutine saxpy (n, sa, sx, incx, sy , incy) 

constant times a vector plus a vector. 

uses unrolled loop for increments equal to one. 

jack dongarra, Unpack, 3/11/78. 

real sx ( 1 ) , sy ( 1 ) , sa 

integer i , incx, incy , ix, iy ,m, mpl , n 

if (n.le.O)return 

if (sa .eq. 0.0) return 

if ( incx . eq. 1 .and . incy . eq. 1 ) go to 20 

code for unequal increments or equal increments 
not equal to 1 

ix = 1 
iy = 1 

if ( incx . It . 0 ) ix = (-n+l)*incx + 1 
if ( incy . 1 t . 0 ) iy = (-n+l)*incy + 1 
do 1 0 i = 1 , n 

sy(iy) = sy(iy) + sa*sx(ix) 
ix = ix + incx 
iy = iy + incy ' 

10 continue 
return 


c 

c code for both increments ecrual to 1 
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clean-up loop 


c 

c 

c 

c 


c 

c 

c 

c 

c 

c 


c 


c 

c 

c 


c 

c 

c 

c 

c 

c 


20 m = mod(n, 4) 

if( m .eq. 0 ) go to 40 
do 30 i = 1/m 

sy (i) = sy (i) + sa*sx(i) 

30 continue 

if { n .It. 4 ) return 
40 mpl = m + 1 

do 50 i = mpl , n, 4 

sy(i) = sy(i) + sa*sx(i) 
sy ( i + 1) = sy (i + 1) + sa*sx(i +* 1) 

sy (i + 2) = sy { i + 2) + sa*sx(i + 2) 

sy (i + 3) = sy ( i + 3) + sa*sx(i + 3) 

50 continue 
return 
end 

subroutine sscal (n, sa, sx, incx) 

scales a vector by a constant. 

uses unrolled loops for increment equal to 1. 

jack dongarra, Unpack, 3/11/78. 

modified to correct problem with negative increments, 

real sa,sx(l) 

integer i , ix, incx, m,mpl , n 

if (n.le.0) return 
if ( incx. eq. 1 ) go to 20 

code for increment not equal to 1 

ix = 1 

if (incx.lt .0) ix = (-n+l)*incx + 1 
do 10 i = 1 , n 

sx(ix) = sa*sx(ix) 
ix = ix + incx 
10 continue 
return 

code for increment equal to 1 


clean-up loop 

2 0 m = mod ( n , 5 ) 

if ( m .eq. 0 ) go to 40 
do 30 i = 1 , m 

sx ( i ) = sa*sx(i) 

30 continue 

if ( n .It. 5 ) return 
40 mpl - m + 1 

do 50 i = mpl , n, 5 
sx(i ) = sa*sx(i) 
sx(i + 1) = sa*sx{i + 1) 

sx ( i + 2) = sa*sx(i + 2) 

sx (i + 3} = sa*sx(i + 3) 

sx ( i + 4) = sa*sx ( i + 4) 

50 continue 
return 
end 

subroutine sswap (n, sx, incx, sy , incy) 


c 

c interchanaes two vectors. 


9/29/88. 
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c uses unrolled loops for increments equal to 1. 

c jack dongarra, Unpack, 3/11/78. 

c 

real sx(l) , sy (1) , stemp 
integer i , incx, incy # ix, iy,m,mpl , n 
c 

if (n.le.O)return 

if (incx.eq.l . and . incy . eq. 1 ) go to 20 
c 

c code for unequal increments or equal increments not equal 

c to 1 

c 

ix = 1 

iy = i 

if ( incx . It . 0 ) ix = (-n+l)*incx + 1 
if (incy . It . 0) iy = (-n+l)*incy + 1 
do 10 i = l,n 
stemp = sx(ix) 
sx(ix) = sy(iy) 

sy(iy) = stemp 

ix = ix + incx 
iy = iy + incy 
10 continue 
return 
c 

c code for both increments equal to 1 

c 

c 

c clean-up loop 

c 

20 m = mod (n, 3 ) 

if( m .eq. 0 ) go to 40 
do 3 0 i = 1 , m 
stemp = sx(i) 
sx { i ) s sy(i) 
sy(i) = stemp 
30 continue 

if( n .It. 3 ) return 
40 mpl = m + 1 

do 50 i = mpl,n,3 
stemp = sx(i) 
sx (i ) = sy (i ) 
sy(i) = stemp 
stemp = sx(i + 1) 
sx(i + 1) = sy(i + 1) 
sy(i + 1) = stemp 
stemp » sx(i + 2) 
sx(i + 2) = sy(i + 2) 
sy(i + 2) = stemp 
50 continue 
return 
end 

integer function isamax(n, sx, incx) 
c 

c finds the index of element having max. absolute value, 

c jack dongarra, Unpack, 3/11/78. 

c modified to correct problem with negative increments, 9/29/88. 
c 

real sx(l),smax 
integer i,incx,ix,n 
c 

isamax = 0 

if{ n .It. 1 ) return 
isamax = 1 
if (n.eq.l) return 
if (incx. ea. 1) ao to 20 
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c 

c code for increment not equal to 1 

c 

ix = 1 

if { incx , It . 0 ) ix = (-n+l)*incx + 1 
smax = abs (sx(ix) ) 
ix = ix + incx 
do 1 0 i = 2 , n 

i f ( abs { sx (ix) ). le . smax) go to 5 

isamax = i 

smax = abs(sx(ix)) 

5 ix = ix + incx 
10 continue 
return 
c 

c code for increment equal to 1 

c 

20 smax = abs(sx(l)) 
do 30 i = 2,n 

if (abs(sx(i) ) .le.smax) go to 30 

isamax = i 

smax = abs ( sx (i ) ) 

30 continue 
return 
end 

subroutine matmul (mata^matb^rod, 11 ,nn,mm) 
real mata(ll,nn) ,matb (nn,mm) ,prod(ll,mm) ,sum 
do 40 i =1,11 
do 3 0 j = 1 , mm 
sum=0 . 0 
do 20 k=l, nn 

sum= sum + mata(i,k) * matb(k,j) 

20 continue 

prod ( i , j ) = sum 
30 continue 
40 continue 
return 
end 

subroutine matadd (mata, matb, sum, rnurnm, code) 
real mata(nn,mm) ,matb(nn,mm) ,sum(nn,mm) 
do 40 i=l,nn 
do 30 j=l,mm 

if (code . ge. 0.) sum(i,j) =mata(i,j) + 

i f ( code .It. 0.) sum ( i , j ) =mata(i,j) - 

30 continue 

40 continue 
return 
end 

subroutine matran (mata , matb, nn, mm) 
real mata (nn,mm) , matb (mm, nn) 
do 40 i=l,mm 
do 30 j=l,nn 
matb ( i , j ) = mata(j,i) 

30 continue 

40 continue 
return 
end 


matb(i, j ) 
matb(i , j ) 
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Discrete LQR Solver 


program dlqr 

c For discrete LQR problem 

c This program uses the reccursive gain formula developed on page 

c 16 of the paper. This program is similar to program Dynam for 

c the dynamic programming approach and uses the same subroutines 

c and therefore this subroutines are not listed again, 

c One only needs to change the dimension of the array in order 

c to run a higher order system 

c Matrices a,h,q are of dimension 1x1 

c matrix b is lxm 

c Matrix f is mxl 

c matrix r is mxm 

c bt, at, are transpose of a and b 

c ati,ri are inverse of at and r 

c dum ( ) are intermediate matrices 

c nos = number of stages 

c 

real a ( 2 , 2 ) , ati (2 , 2) , at ( 2 , 2 ) , q ( 2 , 2 } , p (2 , 2 ) , r (1 , 1 ) , ri (1 , 1 ) 

real b(2,l) r bt (1,2) , dum (2,1), duml (1,1), dum2 (1,2) , dum3 (2,2) , dum3 3(2,2) 

real g(2,2),fk(l,2),u(l,l),x(2,l),h(2,2) ,dum4(2,2) ,dum22(l,2) 

real f 11 (100) , f!2 (100) ,up (1 , 1) , ax (2 , 1) ,bu(2 , 1) ,xkl(2,l) 

open (uni t=3 , f ile= ' dlqr . out ' , status='old' ) 

1 = 2 
m=l 

nos = 10 
c 

c define a,b,q,h matrices 
c 

q ( 1 , 1 ) =50 . 
q ( 1 , 2 } =0 . 
q ( 2 , 1 ) =0 . 
q ( 2 , 2 ) =10 . 
c 

a(l , 1) =0 .6277 
a ( 1 , 2 ) =0 . 3 597 
a (2 , 1) =0 .0899 
a (2 , 2) =0 . 8526 
c 

b ( 1 , 1) = . 025 
b ( 2 , 1) = . 115 
r (1 , 1) =1 . 
c 

h(l , 1) =0 . 
h ( 1 , 2 ) =0 . 
h ( 2 , 1 ) =0 . 
h ( 2 , 2 ) =0 . 
c 

c find constant transpose and inverse 

c 

call matran (b, bt , 1 ,m) 
call matran (a, at , 1 , 1 ) 
call matinv (r , ri ,m, m) 
call matinv (at , ati , 1 , 1 ) 
c 

c set p final to h 

c 

do 10 ii = 1,1 
do 11 j j=l,l 
p (ii , j j ) = h ( ii , j j ) 

11 continue 
10 continue 

do 200 iz=nos,l,-l 

find f(k) = -r A (-l) b A t (a A t) A -l (p-q) 


c 

c 

c 


call matadd (d, cr, dum3 , 1 , 1 , -1 . ) 
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call matmul (ati , dum3 , dum4 , 1 , 1 , 1 ) 
call matmul (bt , dum4 , dum2 ,m, 1 , 1 ) 
call matmul (ri , dum2 , fk,m,m, 1 ) 
c 

c find G (k+1) 

c matrix operation from left 

c 

call matmul (p, b, dum, 1 , 1 , m) 
call matmul (bt , dum, duml ,m, 1 ,m) 
call matadd(r, duml, duml, 111 , 111 , 1 . ) 
call matinv (duml , duml , m, m) 
call matmul (bt / p / dum2 ,m, 1 # 1) 
call matmul (duml , dum2 , dum22 ,m,m # 1) 
call matmul (b, dum2 2 , dum3 , 1 , m, 1 ) 
call matmul (p,dum3, dum3 3,1, 1,1) 
call matadd (p, dum33 , g, 1 , 1 , *1 . ) 
c 

c find pk=a A t g(k+l) a + q 

c 

call matmul (g, a, dum3, 1,1,1) 
call matmul (at , dum3 , dum4 ,1,1,1) 
call matadd (dum4,q,p, 1,1,1) 
c 

c save and decomposed fk 

c 

fll (iz) =fk(l,l) 
fl2 (iz) =fk (1 , 2) 

200 continue 
c 

c calculate optimal control and state trajectories 

c 

c set initial state 

c 

x ( 1 , 1 ) =2 . 

x(2 , 1) =1 . 

do 1000 ij=l,nos 

fk(l,l)=fll(ij) 

fk ( 1 , 2 ) =f 12 ( i j ) 

call matmul ( fk,x, up, m, 1 ,m) 

call matneg (up,u,m,m) 

call matmul (a,x, ax, 1 , 1 ,m) 

call matmul (b,u,bu, 1 ,m,m) 

call matadd (ax, bu, xkl, 1 ,m, 1 . ) 

icount= ij-1 

write (3 ,1001) icount ,x(l , 1) ,x(2,l) ,fk(l,l),fk(l,2) ,u(l,l) 
1001 format (i5,5fl0. 4) 

1000 call matequ (xkl , x, 1 ,m) 
stop 
end 
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Discrete LQR Solver Based on Dynamic Programming 

program dynamic 

c This program uses the reccursive gain formula developed under 

c the multistage decision process (dynamic programming), 

c One only needs to change the dimension of the array in order 

c to run a higher order system 

c Matrices a,h,q are of dimension lxl 

c matrix x is Ixm 

c matrix b is lxm 

c Matrix f is mxl 

c matrix r is mxm 

c bt, at, are transpose of a and b 

c ati,ri are inverse of at and r 

c dum{) are intermediate matrices 

c nos = number of stages 

c This particular program solves the example on page 16 of the 

c paper. 

c One only needs to input the a,q,b,h matices for new problems 

c and change the appropriate array dimension, 

c The basis of this program is the matrix routine developed 

c to perform various matrix manipulations 

c Matrix inverse routine is from LINPAK 
c 

real a (2 , 2 ) , ati ( 2 , 2 ) , at (2 , 2 ) , q (2 , 2 ) ,p(2,2) # r(l,l) , ri (1 , 1 ) 
real b(2,l) ,bt (1,2) ,dum(2,2) ,bpb(l,l) ,rpb(l,l) ,rpbi(l,l) ,apa(2,2) 
real g (2 , 2) , f (1 , 2) ,u(l , 1) ,x(2 , 1) ,h(2 ,2) , bpa(l, 2) , ft (2 , 1) ,duml(2,2) 
real f 11 (100 ) , f 12 ( 100) , up (1 , 1) , ax{2 , 1) ,bu(2,l) ,xkl(2,l) 
open (uni t=3 , f ile= ' dynam. out ' , status= 'old' ) 

1 = 2 
m=l 

nos =10 
c 

c define a,b,q,h matrices' 
c 

q(i, D=50. 
q(l , 2) = 0.. 
q( 2 , 1) = 0 
q(2 , 2) =10 . 
c 

a(l , 1) =0 • 6277 
a(l,2)=0.3597 
a (2 , 1) =0 .0899 
a ( 2 , 2) =0 . 8526 
c 

b(l, 1) =.025 
b(2 , 1) = . 115 
r (1 , 1) =1 . 
c 

h(l,l)=0. 
h(l , 2 ) =0 . 
h(2 , 1) =0 . 
h(2 , 2) =0 . 
c 

c find constant transpose and inverse 
c at is transpose of a 

c ri is inverse of r 

c ati is inverse of at 

c l,m are dimension of matrices 

c 

call matran(b,bt,l,m) 
call matran (a, at , 1 , 1) 
call matinv (r, ri ,m,m) 
call matinv (at , ati , 1 , 1) 


c 

c 

c 


set p final to h 
do 10 ii = 1,1 
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do 11 jj=l,l 
p (ii , j j ) = h ( ii , j j ) 


11 

continue 


10 

continue 

c 


do 200 iz=nos,l,-l 

c 



c 


find f(k) = [R + Bt P B] A 

c 




call matqual (bt ,p, a,bpa,m, 1 , 1) 
call matqua(bt,p,b,bpb,m,l,m) 
call matadd(r,bpb,rpb,m,m, 1 , ) 
call matinv(rpb,rpbi ,m,m) 
call matmul (rpbi ,bpa, f ,m,m, 1) 
c 

c find P(k+1) 

c matrix operation from left 

c 

call mat ran ( f , f t ,m, 1 } 
call matqual (at , p , a , apa ,1,1,1) 
call matqual (ft, rpb, f , dum, 1 ,m, 1) 
call matqual (at , p, a, apa, 1 , 1 , 1) 
call matadd (apa, dum, dum, 1 , 1 , -1 . ) 
call matadd (dum, q, p, 1 , 1 , 1 . ) 
c 

c save and decompose f 

fll(iz)=f(l,l) 
fl2 (iz) =f (1,2) 

200 continue 


c 

c 

c 

c 


1001 

1000 


20 

10 


20 

10 


calculate optimal control and state trajectories 
and set initial state values 

x(l , 1) =2 . 

x(2 , 1) =1 , 

do 1000 ij=l,nos 

f (1 , 2) =f 12 (i j ) 

f <l,l)=fll(ij) 

call matmul ( f,x, up, m, 1 ,m) 

call matneg(up,u,m,m) 

call matmul (a,x, ax, 1 , 1 ,m) 

call matmul (b, u, bu , 1 ,m, m) 

call matadd {ax, bu,xkl, l,m, 1 . ) 

icount=i j -1 

write (3 , 1001) icount , x (1 , 1 ) ,x{2,l) / f(l,l),f(l,2),u(l,l) 

format (i5,5fl0. 4) 

call matequ (xkl ,x, 1 ,m) 

stop 

end 

subroutine matequ (mata, matb, lr, lc) 
real mata (lr , lc) ,matb{lr , lc) 
do 10 il=l,lr 

do 20 i2 = 1,1c 
matb (il , 12 ) =mata (il , i2 ) 
continue 
continue 
return 
end 

subroutine matneg(mata ,matb, lr, lc) 
real mata ( lr, lc) , matb ( lr, lc) 
do 10 il=l , lr 
do 20 i2=l,lc 
matb (il , i2 ) =-mata (il , i2 ) 
continue 
continue 
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20 

10 


c 

c 

c 

c 


c 

c 

c 

c 

c 

c 


return 

end , _ . 

subroutine matqua (mata , matb,matc , matd, mm, 11 ,mm) 
real mata (mm, 11) ,matb( 11 , 11) , mate (11 , mm) ,matd (mm, mm) ,dum(2,l) 
1 = 11 
m=mm 

call matmul (math, mate, dum, 1 , 1 ,m) 
call matmul (mata, dum,matd,m, 1 ,m) 
return 
end 

subroutine matqual (mata , math, mate, matd, mm, 11 ,kk) 

real mata (nun, 11) ,matb(ll,ll) ,matc(ll,kk) ,matd(mm,kk) ,dum(2,2) 

call matmul (matb,matc, dum, 11 , 11 , kk) 

call matmul (mata , dum, matd, mm, 11 , kk) 

return 

end 

subroutine matinv(a, ai,n,m) 
real a(n,m) , ai (n,m) , aa(2,2) 
do 10 i = l,n 
do 20 j=l,m 
aa ( i , j ) =a (i , j ) 
continue 
continue 

call sgefa (aa, n, m, ipvt , inf o) 

call sgedi (aa,n,m, ipvt , det , work, 01) 

. do 1 ii = l , n 

do 2 ij=l,m 
ai (ii , i j ) =aa (ii , ij ) 
continue 
continue 
return 
end 

subroutine sgedi (a , Ida , n, ipvt , det , work, job) 
integer Ida, n, ipvt (1) , job 
real a { Ida , 1 ) , det (2 ) , work ( 1 ) 

sgedi computes the determinant and inverse of a matrix 
using the factors computed by sgeco or sgefa. 


c 

on entry 


c 

c 

c 

a 

real (Ida, n) 

the output from sgeco or sgefa. 

c 

c 

c 

Ida 

integer 

the leading dimension of the array < 

c 

c 

c 

n 

integer 

the order of the matrix a . 

c 

c 

c 

ipvt 

integer (n) 

the pivot vector from sgeco or sgefa 

c 

c 

c 

work 

real (n) 

work vector. contents destroyed. 

c 

c 

c 

c 

c 

job 

integer 

= 11 both determinant and inverse. 

= 01 inverse only. 

= 10 determinant only. 


on return 
a 


inverse of original matrix if requested, 
otherwise unchanged. 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


c 

c 

c 

c 


c 


c 

c 

c 


det real (2) 

determinant of original matrix if requested, 
otherwise not referenced, 
determinant = det(l) * 10.0**det(2) 
with 1.0 .le. abs(det(l)) .It. 10.0 
or det(l) .eq. 0.0 . 

error condition 

a division by zero will occur if the input factor contains 
a zero on the diagonal and the inverse is requested, 
it will not occur if the subroutines are called correctly 
and if sgeco has set rcond ,gt. 0.0 or sgefa has set 
info .eq. 0 . 

linpack. this version dated 08/14/78 . 

cleve moler, university of new mexico, argonne national lab. 

subroutines and functions 

bias saxpy , sscal , sswap 
fortran abs,mod 

internal variables 

real t 
real ten 

integer i , j , k, kb, kpl , 1 , nml 


compute determinant 

if (job/10 .eq. 0) go to 70 
det (1) = l.OeO 
det ( 2 ) = O.OeO 
ten = lO.OeO 
do 50 i = 1, n 

if (ipvt(i) .ne. i) det(l) = -det{l) 
det(l) = a (i , i) *det (1) 

. . . exit 

if (det(l) .eq. O.OeO) go to 60 
10 if (abs(det{l)) .ge. l.OeO) go to 20 

det(l) = ten*det(l) 
det (2) = det (2) - l.OeO 
go to 10 
20 continue 

30 if (abs(det(l)J .It. ten) go to 40 

det (1) = det (1) /ten 
det (2) = det (2) + l.OeO 
go to 30 
40 continue 

50 continue 

60 continue 

70 continue 

compute inverse (u) 

if (mod(job,10) .eq. 0) go to 150 
do 100 k = 1, n 

a (k, k) = 1 . OeO/a (k, k) 
t = -a (k, k) 

call sscal (k-1 , t , a (1 , k) ,1) 
kpl = k + 1 

if (n .It. kpl) go to 90 
do 80 j = kpl, n 
t = a (k , i ) 
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80 

90 

100 


c 

c 

c 


110 


120 


130 

140 

150 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


a(k,j) = O.OeO 

call saxpy (k, t,a(l,k),l,a(l,j),l) 
continue 
continue 
continue 

form inverse (u) ^inverse (1) 
nml = n - 1 

if {nml .It. 1) go to 140 
do 130 kb = 1, nml 
k = n - kb 
kpl = k + 1 
do 110 i = kpl, n 
work(i) = a(i,k) 
a ( i , k) = 0 . OeO 
continue 

do 120 j = kpl, n 
t = work ( j ) 

call saxpy (n,t,a(l,j) , 1 , a ( 1 ,k) ,1) 
continue 
1 = ipvt(k) 

if {1 ,ne. k) call sswap (n, a (1 ,k) , 1 , a (1 , 1) , 1 ) 
continue 
continue 
continue 
return 
end 

subroutine sgef a (a , Ida , n, ipvt , info) 
integer Ida , n, ipvt ( 1 ), info 
real a(lda,l) 

sgef a factors a real matrix by gaussian elimination. 

sgefa is usually called by sgeco, but it can be called 
directly with a saving in time if rcond is not needed. 

(time for sgeco) = (1 + 9/n)*(time for sgefa) . 

on entry 

a real (Ida, n) 

the matrix to be factored. 

Ida integer 

the leading dimension of the array a . 

n integer 

the order of the matrix a . 

on return 

a an upper triangular matrix and the multipliers 

which were used to obtain it. 

the factorization can be written a = l*u where 
1 is a product of permutation and unit lower 
triangular matrices and u is upper triangular. 

ipvt integer (n) 

an integer vector of pivot indices. 

info integer 

= 0 normal value. 

= k if u(k,k) .eq. 0.0 . this is not an error 
condition for this subroutine, but it does 
indicate that sgesl or sgedi will divide by zero 
if called, use rcond in saeco for a reliable 
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n o 


c indication of singularity, 

c 

c linpack. this version dated 08/14/78 . 

c cleve moler, university of new mexico, argonne national lab. 
c 

c subroutines and functions 

c 

c bias saxpy , sscal , isamax 

c 

c internal variables 

c 

real t 

integer isamax, j , k, kpl , 1 , nml 
c 
c 

c gaussian elimination with partial pivoting 

c 

info = 0 
nml = n - 1 

if (nml .It. 1) go to 70 
do 60 k = 1, nml 
kpl = k + 1 
c 

c find 1 = pivot index 

c 

1 = isamax (n-k+1 , a (k, k) , 1 ) + k - 1 
ipvt(k) = 1 
c 

c zero pivot implies this column already triangularized 

c 

if (a(l,k) . eq. O.OeO) go to 40 

c 

c interchange if necessary 

c 

if (1 .eq. k) go to 10 
t = a(l,k) 
a(l,k) = a (k, k) 
a (k, k) = t 
10 continue 

c 

c compute multipliers 

t = -1 . OeO/a (k, k) 
call sscal (n-k, t , a (k+1 , k) ,1) 
c 

row elimination with column indexing 

do 30 j = kpl, n 
t = a (1 , j ) 

if (1 .eq. k) go to 20 
a ( 1 , j ) = a (k, j ) 
a (k, j ) - t 

20 continue 

call saxpy (n-k, t , a (k+1 , k) ,l,a (k+1 , j ) , 1) 

30 continue 

go to 50 
40 continue 

info = k 
50 continue 

60 continue 
70 continue 
ipvt(n) = n 

if (a(n,n) .eq. O.OeO) info = n 
return 
end 

subroutine saxov (n r sa,sx, incx, sv , incv) 


50 


c constant times a vector plus a vector, 

c uses unrolled loop for increments equal to one. 

c jack dongarra, Unpack, 3/11/78. 

c 

real sx { 1 ) , sy ( 1 ) , sa 
integer i , incx, incy , ix, iy,m,mpl , n 
c 

if (n.le.O) return 
if (sa . eq. 0.0) return 
if (incx. eq.l .and. incy. eq.l)go to 20 
c 

c code for unequal increments or equal increments 

c not equal to 1 

c 

ix = 1 

iy = 1 

if (incx . It . 0 ) ix = ( -n+1 ) *incx + 1 
if (incy . It . 0 ) iy = (-n+l)*incy + 1 
do 1 0 i = 1 , n 

sy(iy) = sy(iy) + sa*sx(ix) 
ix = ix + incx 
iy = iy + incy 
10 continue 
return 
c 

c code for both increments equal to 1 

c 

c 

c clean-up loop 

c 

20 m = mod(n, 4) 

if ( m . eq. 0 ) go to 40 
do 3 0 i = 1 , m 

sy(i) = sy(i) + sa*sx(i) 

30 continue 

if( n .It. 4 ) return 
40 mpl = m + 1 

do 50 i = mpl,n,4 

sy(i) = sy(i) + sa*sx(i) 
sy(i + 1) = sy(i + 1) + sa*sx(i + 1) 

sy(i + 2) = sy(i + 2) + sa*sx(i + 2) 

sy(i + 3) = sy(i + 3) + sa*sx(i + 3) 

50 continue 
return 
end 

subroutine sscal (n, sa, sx, incx) 
c 

c scales a vector by a constant, 

c uses unrolled loops for increment equal to 1. 

c jack dongarra, Unpack, 3/11/78. 

c modified to correct problem with negative increments, 
c 

real sa,sx(l) 
integer i, ix, incx, m, mpl, n 
c 

if (n . le . 0) return 
if ( incx . eq . 1 ) go to 20 
c 

c code for increment not equal to 1 

c 

ix = 1 

if (incx. It . 0) ix = (-n+l)*incx + 1 
do 1 0 i = 1 , n 

sx{ix) = sa*sx(ix) 
ix = ix + incx 


9/29/88. 
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10 continue 
return 
c 

c code for increment equal to 1 

c 

c 

c clean-up loop 

c 

20 m = mod (n, 5 ) 

i f { m .eq. 0 ) go to 40 
do 30 i = l,m 

sx(i) = sa*sx(i) 

30 continue 

iff n .It. 5 ) return 
40 mpl = m + 1 

do 50 i = mpl , n, 5 


sx(i) 

= sa 

*sx ( i ) 



sx ( i + 

1) 

= sa*sx(i 

4* 

D 

sx ( i + 

2) 

= sa*sx ( i 

+ 

2) 

sx ( i + 

3) 

= sa*sx(i 

+ 

3) 

sx ( i + 

4) 

= sa*sx(i 

+ 

4) 


; 50 continue 
return 
end 

subroutine sswap (n, sx, incx, sy , incy) 
c 

c interchanges two vectors. 

c uses unrolled loops for increments equal to 1. 

c jack dongarra, Unpack, 3/11/78. 

c 

real sx(l) , syfl) ,stemp 
integer i , incx, incy, ix, iy ,m,mpl ,n 
c 

if (n.le. 0) return 

if (incx. eq.l .and. incy. eq.l)go to 20 
c 

c code for unequal increments or equal increments not equal 

c to 1 

c 

ix = 1 

iy = i 

if (incx. it . 0) ix = (-n+l)*incx + 1 
if (incy . It . 0) iy = {-n+l)*incy + 1 
do 10 i = 1 , n 
stemp = sx(ix) 
sx(ix) = sy(iy) 

sy(iy) = stemp 

ix = ix + incx 
iy = iy + incy 
10 continue 
return 
c 

c code for both increments equal to 1 

c 

c 

c clean-up loop 

c 

20 m = mod(n,3) 

if( m .eq. 0 ) go to 40 
do 3 0 i = 1 , m 
stemp = sx(i) 
sx ( i ) = sy ( i ) 
sy(i) = stemp 
30 continue 

iff n .It. 3 ) return 
40 mpl = m + 1 
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50 


do 50 i = mpl , n, 3 
stemp = sx(i) 
sx { i ) = sy(i) 
sy ( i ) = stemp 
stemp = sx{i + 1) 
sx(i + 1) = sy{i + 1) 
sy(i + 1) = stemp 
stemp = sx(i + 2) 
sx(i + 2) = sy(i + 2) 
sy(i + 2) = stemp 
continue 
return 
end 

integer function isamax (n, sx, incx) 

finds the index of element having max. absolute value, 
c jack dongarra. Unpack, 3/11/78. 

c modified to correct problem with negative increments, 9/29/88. 
c 

real sx(l),smax 
integer i , incx, ix, n 
c 

isamax = 0 

if ( n .It. 1 ) return 
isamax = 1 
i f ( n . eq . 1 ) return 
if ( incx . eq . 1 ) go to 20 
c 

c code for increment not equal to 1 

c 

ix = 1 

if (incx . It . 0 ) ix = (-n+l)*incx + 1 
smax = abs(sx(ix)) 
ix = ix + incx 
do 1 0 i = 2 , n 

if (abs ( sx (ix) ) . le . smax) go to 5 

isamax = i 

smax = abs(sx(ix)) 

5 ix = ix + incx 
10 continue 
return 
c 

c code for increment equal to 1 

c 

20 smax = abs(sx(l)) 
do 30 i = 2,n 

if (abs(sx(i) ) .le.smax) go to 30 

isamax = i 

smax = abs (sx(i) ) 

30 continue 
return 
end 

subroutine matmul (mata ,matb, prod, 11 , nn,mm) 
real mata(ll,nn) ,matb{nn,mm) ,prod(ll,mm) ,sum 
do 40 i =1,11 
do 30 j=l,mm 
sum=0 . 0 
do 20 k=l, nn 

sum= sum + mata(i,k) * matb(k,j) 

20 continue 

prod(i,j) = sum 
30 continue 
40 continue 
return 
end 

subroutine matadd (mata , matb, sum, nn,mm, code) 


53 



real mata(nn,mm) , matb (nn,mm) ,sura(nn,mm) 
do 4 0 1 = 1,1111 
do 30 j =1 ,mm 

if (code . ge. 0.) sum(i,j) = mata(i,j) 

if (code .It. 0.) sum ( i , j ) = mata(i,j) 

30 continue 

40 continue 
return 
end 

subroutine mat ran (mata, matb, nn,mm) 
real mata (ni,mm) ,matb{mm, nn) 
do 40 i = 1 , mm 
do 30 j = 1 , nn 
matb(i,j) = mata{j,i) 

30 continue 

40 continue 
return 
end 


+ matb(i,j) 
- matb(i,j) 
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